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Chapter 1 

Introduction 


This document reports on the work done under NASA Cooperative Agreement NCC 2-333 
during the period March 1987 through August 1987. The research is carried out by a team 
of graduate students comprising the Stanford University Aerospace Robotics Laboratory 
under the direction of Professor Robert H. Cannon, Jr. The goal of this research is to 
develop and test new control techniques for self-contained, autonomous free-flying space 
robots. Free-flying space robots are envisioned as a key element of any successful long term 
presence in space. These robots must be capable of performing the assembly, maintenance, 
and inspection, and repair tasks that currently require astronaut extra-vehicular activity 
(EVA). Use of robots will provide economic savings as well as improved astronaut safety 
by reducing and in many cased eliminating the need for human EVA. 

The focus of our work is to develop and carry out a set of research projects using 
laboratory models of satellite robots. These devices use air cushion technology to simulate 
in two dimensions the drag-free, zero-g conditions of space. Using two large granite surface 
plates (6’ by 12’ and 9’ by 12’) which serve as the platforms for these experiments we are 
able to reduce gravity induced accelerations to under 10~®p with a corresponding drag-to- 
weight ratio of about 10“^ — a very good approximation to the actual conditions in space. 

Our current work is divided into four major projects or research areas: Cooperative 
Manipulation on a Fixed Base, Cooperative Manipulation on a Free-Floating Base, Global 
Navigation and Control of a Free-Floating Robot, and an alternative transport mode called 
LEAP (Locomotion Enhancement via Arm Push-Off). 

The fixed-base cooperative manipulation work represents our initial entry into multiple 
arm cooperation and high-level control with a sophisticated user interface. This experiment 
is just coming on-line and should be fully operational shortly. 

The floating-base cooperative manipulation project strives to migrate some of the new 
technologies developed in the fixed-base work onto a floating base. This experiment will 
be using our second generation space-robot model which is still under construction. 

The global control and navigation experiment seeks to demonstrate simultaneous con- 
trol of the robot manipulators and the robot base position so that tasks can be accomplished 
while the base is undergoing a controlled motion. 

The LEAP project is a new activity that was started during this report period with 
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the goal of providing a viable alternative to expendable gas thrusters for vehicle propulsion 
wherein the robot uses its manipulators to throw itself from place to place. This work will 
be carried out with a slightly revised version of the second generation space robot which 
is currently under construction. 

The chapters that follow give detailed progress and status reports on a project by 
project basis. Also undergoing final preparation is Harold L. Alexander’s PhD thesis 
entitled “Experiments in Control of Satellite Manipulators.” This document will provide 
an in-depth report on the initial satellite robotics work done at Stanford University. 


Chapter 2 

Fixed-Base Cooperative 
Manipulation Experiment 


Stan Schneider 


2.1 Introduction 

To accelerate our development of multi-armed, free-flying satellite manipulators, we are de- 
veloping a fixed-base cooperative manipulation facility. Although the manipulator arms are 
fixed, they will manipulate free-flying objects. By allowing allow us to quickly experiment 
with cooperative algorithms, this facility will greatly expedite our study of space-based ma- 
nipulation and assembly. This section describes the progress made to date in our research 
on cooperative manipulation. 

Progress Summary 

The major activities completed during during the period March, 1987 through August 1987 
were: 

• Constructed the cooperating-axms experimental system 

• Developed multiprocessor real-time computer system 

• Automated the inertia measurement device, and measured arm link parameters 

• Began development of the real-time software 

• Demonstrated simple single-arm control 

Background: research goals 

Space construction requires the manipulation of large, delicate objects. Single manipula- 
tor arms are incapable of quickly maneuvering these objects without exerting large loc«il 
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torques. Multiple cooperating arms do not suffer from this limitation. Unfortunately, co- 
operative robotic manipulation technology is not yet well understood. The goal of this 
project is to study the problem of cooperative manipulation in a weightless environment, 
and experimentally demonstrate a cooperative robotic assembly. 

Four aspects are to be studied in detail: 

• The dynamic control of multiple arm manipulation systems 

• The utilization of video “vision” data for real-time control 

• Real-time software structuring for cooperative robotic systems 

• User interfacing: the acquisition and utilization of strategic commands 

2.2 Facility Development 


During this period, the mechanical hardware and computer system design described in 
the previous report was realized. The fixed-base cooperation facility consists of a pair of 
two-link manipulators, affixed to the side of a “small” granite table (4x8 feet). Each arm 
is of the popular SCARA configuration — basically anthropomorphic, with vertical-axis, 
revolute “shoulder” and “elbow” joints. The arms are capable of motion in the plane of 
the table, and can interact with air-cushion objects floating on the granite surface. 

The arms were designed with two major goals: to be compatible with the SRSV design, 
and to be as simple as possible. The SRSV design constraints are described elsewhere. 

Manipulator design 

Each link is directly driven by a limited angle torque motor. These motors were chosen 
for their nearly frictionless operation. The elbow joint is driven remotely via a cable and 
pulley system. This allows the elbow motor to be located at the shoulder base, and thus 
drastically reduces the effective inertia of the upper-arm link. It also permits a 3:2 gear 
reduction in the elbow drive train, extending the range of motion of the elbow joint. 

Joint angle sensors 

Each joint is equipped with a rotary variable differential transformer (RVDT). These sen- 
sors provide direct angular position measurements. To provide an estimate of the joint 
angular velocity, the position signals are passed through a pseudo-differentiation filter. 
This filter has the transfer function • Thus, at low frequencies (small s), it ap- 

proximates a differentiation operator. Unfortunately, this filter does introduce some 
phase lag, but it is not significant at the low (almost D.C.) frequencies of importance to 
this rigid body system. 


2.3. Computer System 
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Force sensing gripper 

In order to be able to manipulate targets in a cooperative manner we have developed a force- 
sensing end effector. A pneumatic actuator drives a beam assembly in the vertical direction 
providing positive attach and release functions. Strain gauges on the beam provide force 
signals in the two planar directions. In preliminary calibration tests, the device was capable 
of a very high signal-to- noise ratio — on the order of 2000:1. Crosstalk and non-linearities 
in the strain gauges are less ideal. 

Vision system 

Completion of the tasks required of our robots requires sensing not only the robot’s motions, 
but also those of objects in its environment. To allow this, we are developing a simple 
computer vision system. This system is capable of tracking specially designed variable 
gray-scale “targets”. Considerable theoretical work was done (under a previous contract) 
to analyze the sub-pixel tracking performance of a vision system employing these targets. 
An excerpt of that work is attached as appendix A. 

Floating object development 

Our floating air-cushion objects have also undergone evolutionary development since the 
last report. These objects are independent miniature air-cushion vehicles, equipped with 
a small battery powered aquarium-pump air supply. The arms can manipulate them with 
their grippers, thus providing a two-dimensional simulation of space-based manipulation. 

We have developed several prototype models for these objects. The original design 
had oval-shaped pads with several air outlet holes and flow restriction orifices. While this 
design worked under most conditions, we found that the small aquarium pumps developed 
insufficient air flow through the orifices. After several iterations, we have settled on a design 
with only two outlet holes, but with larger air plenums on the lower surface of the pad. 
Dual pumps provide each plenum with an independent air supply. This design permits 
relatively large off-center loading, while still permitting fast object motions on the limited 
air pressure developed by the portable pump. 


2.3 Computer System 


Our real-time computer system combines a proven UNIX development environment with 
high performance real-time processing hardware. Motorola 68020/68881 single-board pro- 
cessors running the real-time kernel pSOS provide inexpensive real-time processing power. 
VME bus shared-memory communications permit efficient multiprocessor operation. The 
real-time processors are linked, via the VME bus, to our Sun/3 en^neeriug workstations. 
Thus, we benefit from Sun’s superb programming environment, while providing the capac- 
ity for relatively cheap, unlimited processing expansion. 
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Real-time hardware 

The fixed-base facility computer system was configured and installed during the report 
period. This system employs three Motorola MVME-133 processor cards. These cards 
each feature a 68020 CPU, a 68881 floating point co-processor, and 1 Mbyte of memory. 
The system communicates with our Sun workstation via a VME bus repeater. 

Both analog and digital interfaces are provided to the robotic hardware. A/D converters 
process the incoming RVDT and force sensor signals. The motors are driven by a simple 
D/A converter and a current amplifier. The pneumatic gripper actuator is interfaced to 
the system via a distal interface card. 

Real-time software 

We are actively developing a software link between our Sun workstations and the real-time 
computer system. We have successfully utilized the Sun’s native “C” compiler to produce 
and down-load real-time system code. A more general and powerful real-time programming 
environment is under development. 


2.4 Calibration 


The lab’s inertia measurement device was automated during this report period, and all 
arm inertia and mass parameters were measured. This section describes the calibration 
methodology. 

The ARL’s inertia measuring device is a simple 3-wire rotary pendulum, with a plate 
to hold the measured part. The pendulum’s period is related to the inertia of the part on 
the plate. The pendulum and its dynamics have been described in previous reports. 

In the past, the pendulum period had been meaisured manually, and rather tedious 
calculations were required to calculate the unknown part’s inertia. By using an LED and 
photodiode to sense the motion of the swinging pendulum, the real-time system can calcu- 
late the pendulum’s period. The manual process has been replaced by a simple automated 
sequence: After the device is calibrated with two know inertias, unknown inertias may 
be quickly measured. All calculations are done automatically by the inertia measurement 
program. 


2.5 Controllers 


We are examining interfaces between the dynamic forces and motions of the robotic manip- 
ulators, and higher-level strategic inputs. As a first attempt, we are investigating the appli- 
cation of NeviU Hogan ’s[2] impedance control concept to multi-arm — and multi-vehicle — 
cooperative tasks. Impedance control is very attractive for cooperative tasks because it 
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allows direct control of the interaction between the cooperating agents; control of the me- 
chanical power flow from manipulation system to environment. The implementation for 
multiple arms, however, is not well understood. 

We were successful in implementing a simple Position-Derivative (PD) controller for a 
single arm during this report’s period. This accomplishment demonstrated the viability of 
all the sensor and actuator sub-systems. It also underlined the need for extensive calibration 
and computer software efforts. 

2.6 Future Work 

During the next period, work will continue on the construction and calibration of the 
cooperating arms experimental system. This should allow development of much more 
sophisticated dynamic control algorithms. We will also continue our software development. 
The second arm should be operational soon, and we expect to implement our first dual-arm 
controller during the next report period. 
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Chapter 3 


Multiple Arm Cooperation on a 
Free-Flying Robot 

Ross Koningstein 

3.1 Introduction 

This chapter summarizes the work performed on multiple arm cooperation on a free-flying 
robot. This work represents one of the basic technologies required for space based manipu- 
lation. One of the first steps to achieving control of a system is to understand its dynamic 
properties. This section and an included appendix discuss the work done in the dynamic 
modelling of a typical space robot configuration: the kinematic chain. 

3.1.1 Motivation 

To achieve fast, precise control of a physical system, accurate dynamical modelling is re- 
quired. As controlled dynamical systems become increasingly complex, manual derivation 
by an analyst of the system equations of motion becomes very costly, and human error 
limits the rate at which a dynamical model can be created. Computer codes for gener- 
ating dynamical equations of motion have appeared, however, these codes have neglected 
the needs of the control system designer since they approach the problem purely from a 
simulation viewpoint. Automatic equation generation packages are available, however, un- 
fortunately, in order to control a dynamical system, the system’s Jacobian, J as defined 

by 


^endpoints 


= Jq 


where v is a vector of the speeds of the manipulator endpoints, measured in some coordinate 
system and q are the derivatives of the system generalized coordinates. The Jacobian and 
its derivative need to be used for computation of the desired controls. We will demonstrate 
that the Jacobian can be computed from the same terms used to compute the dynamical 
equations. 
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A secondary motivation is that the dynamical system under study, a dual arm satellite 
manipulator model, is essentially a serial chain of rigid bodies and undergoes only minor 
changes (in terms of structure) when it grasps an object: it either becomes a longer chain 
or it becomes a closed chain. If the equations of motion of a chain system have a certain 
form, then the addition of extra links to the system should result in a small change in the 
computation of the equations of motion and should not necessitate the rederivation of the 
system’s equations of motion from scratch. Generating equations of motion and control 
algorithmically is desirable since this task is then no longer a manual procedure. It will 
require significantly less of the analyst’s time and will be less susceptable to error. 

3.2 Dynamical Modelling of Free-Flying Kinematic Chains 

The space robot being considered falls into the class of objects called kinematic chains. The 
mathematical model for kinematic chains has a special structure allowing an algorithm to 
formulate dynamical equations for arbitrarily long chains. This algorithm can be highly 
efficient, and much of the work done for the dynamic modelling can also be used for control 
specifications and dynamic constraint equations if the chain becomes closed. The theory 
for serial chain manipulators is derived using Kane’s dynamical analysis techniques[3]. The 
theory showing the formulation of equations of motion, the system Jacobian, the control 
specifications and closed loop constraints is outlined in appendix B. Using the algorithm 
developed, it is possible to simiilate kinematic chains of arbitrary size by specifying the 
masses and inertias of the bodies in the chain as well as their interconnections. No further 
work by the analyst is necessary. Note that the algorithm developed to date is not for 
general dynamical systems: it does not handle closed chains nor does it handle three 
dimensional systems. It does, however, provide us with a very powerful tool for the study 
of dynamical chain systems, an example of which is the satellite simulator robot on which 
we wish to test control algorithms for free-flying robots. 

Simulation runs of a dual armed two-dimensional free-flying robot were performed. The 
correctness of the algorithm for dynamical modelling was confirmed both by checking the 
conservation of the system’s Hamiltonian and by comparison to runs of similar equations 
produced by SDEXACT[6]. 

3.3 Status 

Equations of motion for a dual-armed free-flying satellite robot simulator have been derived 
manually and numerically formulated using an algorithm which computes terms of the 
equations of motion. The addition or removal of links of the robot (e.g. to simulate system 
configuration before and after object catches) requires changing only the kinematic chain 
description data structure. 

The algorithm developed has been used to generate numerical dynamical equations for 
the free-flying two armed robot. These equations were solved, and the resulting history of 
coordinates and speeds provided an accurate simulation of the robot’s dynamics. To date, 
only free (uncontrolled) motion of the satellite robot model has been simulated. 
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3.4 Further Research 

The speed of the numerical solution to the numerical dynamical equations can be improved 
for large systems (e.g. three dimensional multi-armed robots) by using a different type 
of numerical dynamical equation. The algorithm derived is an order algorithm for 
simulation and control. We also wish to investigate the newer the order n simulation 
algorithms being developed [5]. The mastering of this theory will allow us to approach 
the control problem of the free-flying robot with fast and accurate dynamics and control 
specification model. Our long run goal is to test this theory on a two-armed two-dimensional 
satellite robot manipulator model in order to investigate the limitations of such theory in 
real-world situations. 


[ 
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Chapter 4 


Navigation and Control of 
Free-Flying Space Robots 

Marc Ullman 

4.1 Introduction 

This chapter summarizes the progress to date in our research on global navigation and 
control of free-flying space robots. This work represents one of the key aspects of our 
comprehensive approach to developing new technology for space automation. Ultimately, 
we envision groups of fully-self contained mobile robots serving as the core work force in 
space. 


4.1.1 Motivation 

Although space presents us with an exciting new frontier for science and manufacturing, 
it has proven to be a costly and dangerous place for people. Space is therefore an ideal 
environment for sophisticated robots capable of performing tasks that currently require the 
active participation of astronauts. 

4.1.2 Research Goals 

The immediate goals of this project are to: 

• demonstrate the ability to simultaneously control vehicle position and arm orientation 
so that a robot can navigate to a specified location in space while manipulating its 
arm(s). 

• demonstrate the ability to capture a (possibly moving) free-floating target “on-the- 
fly” using the manipulator arm while the base is in transit. 

• provide a suitable platform for the eventual addition of A.I. based path planning and 
obstacle avoidance algorithms which will enhance the robustness of task execution. 


preceding page blank not fUMED 
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4.1.3 Background 

This work emphasizes the modeling of robot dynamics and the development of new control 
strategies for dealing with problems of: 

• a non-inertially fixed base (i.e. free-floating base) 

• redundancy with dissimilar actuators 

• combined linear and non-linear actuators 

• highly non-linear dynamics 

• unstructured environments 

Our laboratory work involves the use of a model satellite robot which operates in two- 
dimensions using air-cushion technology. We have developed a series of satellite robots 
which, in two dimensions, experience the drag-free and zero-g characteristics of space. 
These robots are fully self-contained vehicles with onboard gas supplies, propulsion, electri- 
cal power, computers, and vision systems. The latest generation of robots is also equipped 
with a pair of two-link arms for acquiring and manipulating target objects. 


4.2 Summary of Progress 

We have made important strides during the past six month period on both the theoretical 
and experimental fronts. In the area of hardware development, the new robot vehicle 
whose design was described in our last report has continued to materialize. We have 
completed construction and demonstrated operation of the onboard thrust subsystem. We 
have also had a pair of the manipulator arms fabricated and instrumented. These arms 
are now mounted on the robot. A successful closed loop controller has been demonstrated 
in simulation which simultaneously controls the vehicle position (using on-off-on thrusters) 
and the arm orientation (using torque motors). 

4.3 Experimental Hardware 

The current generation robot has been designed to accommodate one or two two-link rigid 
arms mounted between the first and second layers as described in our previous report. 
These arms have been designed and fabricated and now await final wiring of sensors and 
motors. These arms are identical to those used in the fixed-base cooperating-arms exper- 
iment and are described in greater depth in Chapter 2. This hardware commonality will 
greatly simplify the transfer of technology from fixed based to free-floating robots. 


4.4 Modeling and Simulation 

The complete dynamical equations of motion have been derived and verified for a single- 
armed version of the robot. These equations haved been coded up and simulated for both 
free and forced motion. 


4.4. Modeling and Simulation 
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4.4.1 Analytical Model 

The robot has initially been modeled with only one arm since the global control and target 
capturing problems can be addressed with this somewhat simpler configuration. (See the 
section on Multi- Arm Cooperation for a derivation of the equations of motion for the two- 
armed version.) The model consists of three planar rigid bodies connected by two torque 
motors. (See Figure 4.1). The base body is capable of translation and rotation in the plane 
via eight on-off-on thrusters mounted as 90® opposed pairs on each of four corners. 



Figure 4.1: Free body diagram of space robot indicating nomenclature used for 
dynamic modelling. 


4.4.2 Equations of Motion 

The equations of motion for this five-degree-of-freedom system were derived using Kane’s 
method[3] and for verification purposes were also derived using the symbolic equation 
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generation program SDEXACT[6]. The equations are in the form 

Fr + f ; = 0 


or 


F — Af(q)u — V(q, u)u = 0 

where 


F’ = -(M(q)u-|-F(q,u)u) 

M(q) is the configuration dependent mass matrix, F(q, u) is the configuration and velocity 
dependent matrix of non-linear terms, and F is the vector of generalized active forces. The 
u’s or generalized speeds are defined in terms of the state derivatives, q, by the relation 


u = yq 


where 


y = 


1 0 0 0 0 
0 10 0 0 
0 0 10 0 
0 0 110 
0 0 111 


In order to implement a simulation, we must solve the previous set of equations to 
obtain 


q = y-^u 

u = M(q)"^ (F - y(q, u)u) 

The vector of generalized active forces is composed of the net forces and torques applied 
to system and is given by 


F = 


v/here 
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and 


The i?’s are the forces produced by the eight thrusters while the T’s are the torques 
produced by the shoulder and elbow motors respectively. 

A listing of the assumed mass and length parameters is given in Table 4.1. 
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Table 4.1: Assumed Mass and Inertia Properties Used in Simulation 


Mass of Base 

Mb 

110 Ibm 

50 kg 

Mass of Upper Arm 

Ml\ 

2.2 Ibm 

1 kg 

Mass of Fore Arm 

Ml2 

2.2 Ibm 

1 kg 

Radius of Base 

Rb 

9.5 in 

.24 m 

X-axis Base CM Offset 

io. 

1 in 

.025 m 

Y-axis Base CM Offset 


2 in 

.051 m 

Length of Upper Arm 

Lr 

12 in 

.30 m 

X-axis Upper Arm CM Offset 

Lu 

6 in 

.15 m 

Y-axis Upper Arm CM Offset 


1 in 

.025 m 

Length of Fore Arm 

L2 

12 in 

.30 m 

X-axis Fore Arm CM Offset 

L2. 

6 in 

.15 m 

Y-axis Fore Arm CM Offset 

L2, 

1 in 

.025 m 

Moment of Inertia of Base 


5 in 

.13 kg m''^ 

Moment of Inertia of Upper Arm 

Ihlrr 


.13 kg m'^ 

Moment of Inertia of Fore Arm 

h2,. 


.13 kg m''^ 

Thruster Action Line Offset 

rjet 

5 in 

.13 m 


4.4.3 Computer Simulation 

The equations of motion derived above have been coded in C for computer simulation. The 
simulation activity was carried out using the matrix manipulation program PC-Matlab[4]. 
C and Fortran subroutines can be dynamically invoked from within PC-Matlab allowing for 
very rapid algorithm prototyping and development. Two integrators were used to check for 
numerical accuracy, namely a two-evaluation improved Euler method and a four-evaluation 
fixed-step Runga-Kutta method. 

4.4.4 Computed Torque Controller 

A computed torque[l] controller was implemented as a first cut at closed-loop control. The 
input commands were all of the form of time and amplitude scaled unit step fifth order 
“minimum jerk” polynomial trajectories in state (joint) space. Fifth order polynomials 
were selected so that all trajectories began and ended with zero velocity and acceleration. 
These are of the form 


qj = A(6 t® - 15r^ -f lOr^) 


qj = — (30r^ - eOr® + SOr^) 

qrf = ^(120r3 - 1807* 60r) 

V 

where A is an amplitude scale factor and t = t/tj is normalized time. 
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The computed torque controller views the plant as a set of n decoupled second order 
systems and produces a vector of required accelerations T given by 

T = qd + A'„(qd - q) -I- i^p(qd - q) 

where Kj, and are matrices of position and velocity feedback gains. The required 
acceleration T is then fed though an inverted model of the plant dynamics (thus giving 
rise to the alternate name “inverse dynamics”) to produce the required generalized active 
forces (i.e. the computed torques). 


F = M(q)T-|-y(q,u)u 


where M(q) and V'(q, u) are the mass and nonlinear matrices cited above. Since F is an n 
X 1 or in our case a 5 x 1 vector and we have 10 actuators (eight thrusters and two torque 
motors) we observe that, mathematically speaking, our system has redundant degrees of 
freedom, i.e. it is underconstrained. 

Since each thruster can produce only a positive force (i.e. push rather than push or 
pull) we can pair them off in four sets which are capable of producing signed forces. (See 
Figure 4.2) This leaves us with six actuators so we still have one redundant degree of 
freedom. This redundancy issue is resolved using the classic pseudo-inverse technique. We 
have 
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or Crf with the second, fourth, sixth, and eighth columns removed which leads to 


R' - C\ip 


,F 


where f indicates the Moore-Penrose pseudo-inverse[7]. 

Since the thrusters are on-off devices capable of delivering only one force level, we must 
pass R' through a thresholding function as shown in table 4.2. This function also maps 
the four bi-directional thrusters back into eight uni-directional devices. Figure 4.3 shows 
a block diagram representation of the complete closed loop simulation with the computed 
torque controller. 


4.5 Summary 

Progress on robot construction has been moving forward. Now that the robot has arms, 
the bulk of the mechanical work is complete — the arms still need end effectors and we will 
probably use the same design as is being used in the fixed-base work described in Chapter 
2 . 


4.5. Summary 
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Satellite robot with 8 uni-directional Equivalent satellite robot with 4 bi- 
thrusters directional thrusters 

Figure 4.2; Modelling uni-directional thrusters with their bi-directional equivalents 


Table 4.2: Thruster Thresholding and Mapping Function 


R\ > Rih 

Ri 

Rj < —Rth 

R2 

R3 > Rth 

R3 

R{ < Rth 

R4 

R's > Rth 

Rs 

A 

1 

Rb 

R'f > Rth 

Ri 

R's < ~Rth 

Rs 


The control methodology presented above looks promising and we are anxious to able 
to test it using the actual robot hardware. It appears that it is a general algorithm and 
should be easily extensible to arbitrary sets of thrusters including those out of the plane 
(i.e. the 3-D case). 
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4.6 Future Work 

Now that the arms are nearly complete, our emphasis can shift to the robot’s electrical 
systems which make up the “analog layer” of our modular design. This layer which was 
described in detail in an earlier report will contain replaceable battery packs, DC-to-DC 
power converters, and an analog card cage containing sensor and driver electronics, power 
distribution and control circuitry, and safety monitoring and cutout devices. 
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Chapter 5 


Locomotion Enhancement via 
Arm PushofF (LEAP) 


Warren J. Jasper 
Roberto Zanutta 


5.1 Introduction 

To perform complex assembly tasks, an autonomous vehicle needs to move from one place 
to another. The use of propellants may not be ideal because of cost and safety factors. Also, 
the use of thrusters may disturb the environment by impacting a target which the robot is 
trying to grasp. Our alternative approach is called LEAP: Locomotion Enhancement via 
Arm Pushoff. In LEAP, the vehicle pushes itself off from a large space object and “leaps” to 
the desired resting place or simply “crawls” along an object. This is the common mode of 
locomotion used by the astronauts while in the Space Shuttle. This new project was added 
to investigate the problems and issues involved in autonomous space locomotion. The first 
phase of the project involves: devising the experiment, deriving the equations of motion 
and candidate control laws, and then simulating the model to size physical parameters 
for the actual experiment. The second phase encompasses design and fabrication of the 
vehicle, while the third phase is to experimentally verifiy the theoretical development. The 
following paragraphs describe the progress onphase one. 

5.2 The Experiment 

A new air-cushion vehicle is being designed to study LEAP. This vehicle should simulate 
the motions that an autonomous space robot would perform while in the space station or 
maneuvering out in space. The experiment wUl consist of the vehicle pushing off a bar 
located on one side of the granite table, rotating 180 degs, and catching itself by grasping 
a bar located at the other end of the table. Ideally, one would like to complete this task 
without the use of thrusters. However, at the point of initial release from the bar, errors 
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in the velocity of the center of mass of the vehicle can only be corrected using thrusters. 
To enhance the robustness of this approach, thrusters will be incorporated into the control 
laws for midcourse correction. Figure 5.1 shows the robot in three configurations: pushing 
oif the bar, rotating, and catching itself at the other end. By incorporating crawling and 
leaping, the robot can position itself anywhere on the table with a minimum amount of 
propellant. This investigation complements current work done at the Stanford Aerospace 
Robotic Laboratory by incorporating global navigation and object manipulation into a 
general study of locomotion. 


5.3 Vehicle Modelling 


A great deal of design work has been done under this NASA contract in the field of multiple- 
arm autonomous vehicles. Because the underlying philosophy of our current robots allows 
for flexibility in design, the new vehicle will need only a few design modifications. These 
changes include the addition of a momentum wheel subsystem and grippers. Figure 5.2 
shows a schematic view of the LEAP robot used for simulation. This vehicle has eight 
degrees of freedom, one more than the previous design, due to the addition of a momentum 
wheel. 

The momentum wheel is necessary to perform rotation without the use of thrusters. 
This reduces the propellant cost and provides linear control authority for rotation. Unfor- 
tunately, one can not obviate the need for thrusters completely because they are necessary 
for midcourse corrections and deceleration in free space. Also, a new gripper design is 
needed which incorporates force sensing and compliance when grasping an object. 




5.4. Simulation 
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Figure 5.2: Schematic Drawing of the Robot 


5.4 Simulation 


The simulation is used as a design tool to derive specifications for the motors and the 
sensors as well as evaluating different control schemes. After the initial experiments, a 
further iteration of the simulator will be done to increase its fidelity and to incorporate 
second-order effects. Using Kane’s method, the equations of motion were derived in a 
similar manner as those described in Appendix C. These equations were incorporated into 
the non-linear dynamic simulation. In addition to plotting the various states of the robot, 
the computer, a Sun 3/160 workstation, can also produce a real-time graphics display. This 
two dimensional display allows one to understand how the entire system is responding to 
various control laws. 

To simulate the experiment, we implemented the task of leaping from one end of the 
granite table to the other as a finite state machine by dividing the entire task into ten sepa- 
rate states. In each state, the computer uses the appropriate set of boundary conditions and 
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control laws to achieve a desired trajectory. The control laws used are proportional deriva- 
tive control (PD), proportional integral derivative control (PID), and computed torque. 
These control laws are modified to account for discontinuities in the state due to changing 
boundary conditions. These conditions occur when the robot lets go of the bar and goes 
from a nonholonomic closed-loop chain configuration with four degrees of freedom to a 
holonomic free floating configuration with eight degrees of freedom. A similar condition 
arises when the robot grasps the bar at the other end of the table, however, there is always 
the possibility that both arms will not grasp simultaneously, and thus we allow for two 
additional conditions. 


5.5 Future Work 

With the completion of the first phase, we plan to proceed to the second phase of the project 
which includes fabrication and integration of the various subsystems into a working vehicle. 
This phase should last well into the Fall of 1988. Also in the second phase, we will look 
into new sensor technologyfor force and rate sensing and incorporate these sensors into the 
vehicle. 


Appendix A 

Accurate Real-time Vision Sensor 


Stan Schneider ^ 

A.l The Vision Sensor Problem 

Advanced end-point control of complex systems requires tracking several objects simulta- 
neously. End-point information to be used for control feedback must be highly accurate, 
and available at a high sampling rate. To provide this information, we are developing a 
vision system capable of tracking at least four points with an error of less than one part 
in one thousand. Sample rates will be at least 60 times per second per target. No sys- 
tems currently commercially available provide sufficient performance. This report describes 
progress to date in the development of this vision sensor. 


A. 1.1 Proposed Solution 

Recent advances in CCD camera technology have made pixel-based end point detection 
feasible. At the current time, 440 by 240 pixel arrays are available with sampling rates 
greater than 60 Hz. Coupled with local processing power to provide an intelligent interface, 
this allows sample rates well within the above specifications. Unfortunately, the accuracy, 
240:1, is not acceptable for precision control. The problem thus becomes one of gleaning 
sub-pixel accuracy from the available data. 

By utilizing the available gray scale resolution of the camera, this is practicable. In- 
stead of simple binary (white dot or LED) targets, we propose to use targets that vary 
in reflectivity, from black at the edges to near white in the center. They span an area of 
approximately 8 by 8 pixels. Thus, each pixel in the 8x8 grid contains information about 
the location of the center. (In the noise-free case, one could calculate the distance to the 
center from each pixel exactly.) A simple, fast centroid (center-of- brightness) calculation 
then yields an estimate of the actual center. 

‘Summary of work begun in 1986/1987 under AFOSR contract. The vision sensor system described 
herein will be developed under the current NASA contract, and incorporated into the fixed-base cooperative 
manipulation experiment. This appendix is included for completeness. 
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A. 1.2 Summary of Progress 

A probabilistic analysis has been completed. In addition, a full simulation of the target 
location procedure was implemented to verify the analysis. Finally, a simple experimental 
system was constructed, and its performance measured. The results are encouraging. 
With the new camera that has been ordered, the above performance specification should 
be achievable. 

A. 2 Theoretical Analysis 

For the purposes of this discussion, we assume an iV x TV pixel sub-grid. This grid is 
sufficient to encompass the entire target area. The center of the target is calculated as the 
centroid of illumination. We will calculate the centroid in one dimension only, the other is 
identical. 

Let: 

Pij represent the i, pixel, 

S = ^iPij be the weighted pixel illumination, and 

T = Pij be the total illumination. 

For our purposes, u;, is simply the distance from the center of the coordinate system, 
Wi = i - io- 

The centroid is found simply by: 

(A.1) 

Quantization effects are insignificant in comparison with the camera noise. Thus, the 
noise can be assumed to be gaussian, and white. Now, if each pixel is corrupted by 
independent gaussian noise of variance <t^, then both S and T are gaussian random variables 
with: 



(A.2) 



(A.3) 

i,j 


(A.4) 


(A.5) 




We now develop an expression for the probability density function of the centroid. The 
distribution function is found by integrating over the region of s/t < z: 


Where 


rO roo TOO rzt 

F z{,^^ — I I fts(tjS^dsdt-\- I I fts(^t,s')dsdt 
J—oo.Jzt Jo J—oo 




(A.6) 

(A.7) 


A.3. Simulation 
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Using the relation g{x)dx = g{—x)dx and Liebnitz’s rule for differentiation, we 

obtain: ^ ^ 

~ Jo (A-8) 

Substitution of A.7 yields: 

fg(z) = K[ l°° ^ ^^{At^+Bt+C)dt'^ 

Jo Jo 

(A.9) 

with: 



(A.10) 

B = ^^.^^.^{2fita]-{-2ziiga]) 

(A.ll) 

C = ^^^^^{fi]a] + fi]a]) 

(A.12) 

j.' 1 * 

B = T 

27r<T(<7j 

(A.13) 

After integration and collection of terms, the density function is: 


/.(.) = Ar[_ + e‘-7T^ *(^)] 

(A.14) 


Here, $(i) is the error function, 

2 r°° 2 

$(i) = I e“® dx 
yTT Jo 


Note that further simplifications are possible. For example, ^ can be reduced 

to 

In the zero mean ca.se, C = B = 0, this reduces to the well known ratio of gaussian 
densities, the Cauchy density. An interesting result is that the best estimate of the centroid 
is not simply the above ratio of S/T, but the much more complicated expected value 
of fz{z). A rough evaluation of the actual best target center estimate can be made by 
substituting reasonable values for all the parameters (which yields fit is large, /i, is nearly 
zero, and erg is significantly larger than at). This rough analysis yields two results: 1) S/T 
is a very good estimate of the maximum, and 2) the actual best estimate is always slightly 
smaller magnitude (closer to zero) than SfT. 

A. 3 Simulation 

To verify the above analysis, a full simulation of the target center calculation was imple- 
mented. This program allows rapid variation of such variables as: pixel signal-to-noise 
ratio, quantization effects, target size, target reflectivity profile, and background illumina- 
tion variation. 
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A. 3.1 Simulation Description 

The target is modeled as a truncated cone of variable intensity. Figure A.l depicts a typical 
target reflectivity profile. All labeled parameters are user-selectable. 

Target locations are selected randomly near the center of the grid. The camera CCD 
response is calculated by integrating the target illumination over each pixel. Gaussian noise 
is added, and the value quantized to the user-specified number of levels. The target location 
is then determined by the above algorithm. The program outputs raw data, average error, 
and an error histogram. 



Figure A.l: Simulated Target Reflectivity Profile 


A. 3. 2 Simulation Results 

The graph in figure A.2 shows a plot of the histogram of error values obtained over 1000 
trials. The above probability density function is also plotted for comparison. Under realistic 
conditions (100:1 signal/noise amplitude; 3 pixel radius linear density targets), the method 
located the center to better than 1/lOth of a pixel worst case, l/20th pixel average case. 
With a 440x240 pixel camera, this represents very respectable accuracy (typically 1 part 
in 2000!). 

The centroid calculation algorithm could be improved slightly. Due to the division 
nonlinearity, the best estimate of the true centroid is not simply S/T, but a value slightly 
smaller. Use of a simple scaling factor could reduce the frequency of the larger errors. In 
the case of this system, it is unlikely this effect will be significant. 


A. 4 Preliminary Experimental Results 

Before ordering high speed equipment, a preliminary proof-of-concept experiment was con- 
ducted. Using an IBM PC with a conimercially available low-speed frame digitizer, data 
from a standard format camera was collected. Figure A. 3 shows a representative target. 



A. 5. Summaury 
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Figure A.2; Simulated Error Histogram 

The centroid calculation algorithm was implemented with no attempt at real time perfor- 
mance. 

The camera produced 256 lines of data over an 80 inch field of view. Several measured 
target locations were chosen, and the results analyzed for both stability and accuracy. In 
aU cases, stability was reasonable; jitter was never greater than .05 pixels (.02 inches). 
Also, the accuracy was acceptable. The center of the target was consistently found with 
less than a 0.16 pixel (0.05 inches) error. Linearity over larger distances was as good as 
the lens optics: about 1% over the field of view. Mike Hollars (a student in our lab) has 
developed a compensation scheme for optical non-linearities that shoidd solve this problem. 


A. 5 Summary 

In summary, this technology promises significant advantages over the photodiode position 
sensors currently employed by the lab. This system should provide: 

• Multiple target tracking - at least 4 targets. 

• High accuracy - at least 1000:1. 

• High speed - The system should provide 60-1- Hz samples. 

• An intelligent interface - freeing the main control processor from low-level sensor 
data processing. 



Appendix A. Accurate Real-time Vision Sensor 



Figure A.3: Example Target 

• Future flexibility - Since the entire scene is available to the image processing system, 
further functionality could be implemented. For instance, objects in the environment 
could be identified to be used in assemblies, or mapped to be avoided. Use of simple 
scene analysis could also greatly benefit future cooperative tasks. 


Appendix B 


Serial Chain Manipulator 
Equations of Motion 


Ross Koningstein 

This theory for serial chain manipulators is derived using Kane’s dynamical analysis 
techniques. The analysis that follows assumes that the velocities v of points and angular 
velocities uj of bodies in the system under consideration can be expressed in a Newtonian 
reference frame as follows: 


p 



v’ 

= Hv’u, 

5=1 

P 

• 


II 


This will be true if no part of the system is undergoing prescribed motions. The partial 
angular velocities of bodies, and partial velocities of points, as defined by Kane[3], can be 
shown to be: 


Vr = 

= 

This analysis will show that the elements 

M« = — Nu + F 

are highly structured and can be computed numerically with a very straightforward al- 
gorithm. In order for the algorithm to function, the partial velocities of the first chain 
element, and the partial angular velocities of all chain elements need to be specified. Man- 
^ ual computation of accelerations, and force components is not required. These derivations 

assume all velocities, angular velocities, momenta are expressed in a Newtonian reference 
frame. 


d 

duj 

d 

dUr^ 

in the matrix equation 
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B.l Structure in Dynamical Equations 

In the Kane’s Dynamical equations 


F, + f; = 0 


The generalized active forces are 


T^= F‘ -v‘’+ ^ 

Applied Applied 

force* i torque* j 

and that the generalized inertia forces are 

F;= YL -^r + Y • “’r 

Applied Applied 

force* i torque* j 

The generalized inertia force expression, when derived for serial chains, will be examined 
more closely. Expressions for the inertial forces will be derived using the linear and angular 
momenta of the bodies in the system. First, the terms due to changes in linear momentum 
will be examined, then terms due to changes in angular momentum will be examined. 

The linear momentum of the body is defined as 


L* = miv’ 


= m 


«=i 

0=1 


0=1 


where the partial linear momentum is defined by 

t — 

The inertia force caused by the acceleration of the center of mass of the body is: 




= -ELX-^Lji, 


a=l s=l 

Its contribution to the generalized inertia forces is 


F’* • V* = • vi-u. 


0=1 


0=1 


B.l. Structure in Dynamical Equations 
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The contribution of the changes in angular momentum will now be examined. The 
angular momentum of the body is defined as 

H’ = 

s=l 

a=l 

= Eh>. 


*=1 


where the partial angular momentum is defined by 

Hi = r/‘'a,i 

The inertia torque caused by the angular acceleration of the body is: 


T'* = -tH* 
dt 


= -5:Hx-5:Hiu. 


*=i 

Its contribution to the generalized inertia forces is 


T'* • wi = x: Hi . c^iu, + ^ Hi • 


«=i 


a=l 


The generalized inertia forces can be then be expressed as 


f; = - E (ELi-vix + ELi-vi-u. 

All \3=1 8=1 

Bodie«i 

All . . Xszzl 3=1 


Bodlesi 


The generalized inertia force can be separated into two components, one encompassing 
the derivatives of the generalized speeds, and the other comprising all the rest. 


F" = Eh; 

All 5=1 5=1 

Bodieti 

ELi-v>,-X:Hi.a,X 


All «=1 
Bodiesi 


a=l 
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The inertia and momentum scalars which make up the mass matrix and non-linear 
coupling matrix can then be evaluated as follows: 


mra 

A 

E 

All 

Bodie* : 


•vr-ii 


)-• 

^TS 

A 

E 

All 

Bodies i 

m.-v” 

t 


(m* X I* 

8 


The generalized inertia forces can be constructed as follows: 



where 

n 

a=l 

asl 

The mass matrix and non-linear coupling matrix can then be used to express the equa- 
tions of motion of the system as: 


Mil = — Nw -f F 

where F is the generalized active force vector which accounts for the effects of external 
forces and torques applied to points and bodies in the system: 

F,= Y. v^F^-l- Y 

All exlernftl All exl«rDftl 

force* torque* 

B.2 Kinetic Energy and Power Input 

In a system as discussed in the previous subsections, the kinetic energy can be expressed 
as: 
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^ 4ll \r=l / \5=1 

bodie«i 


+ ; 
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) ■ * 1 

fe-'-) 


= 5 E EE"‘X' 


4ll r=l «=1 
bodiesi 


+7 E EE>^; 


«U r=l <=1 
bodtesi 




Ail r=l 5=1 
bodt«Al 


= u^Mu 

The power input to the system as discussed is due to work done by the actuators (arm 
torquers), and forces exerted on the manipulator endpoints during contact with external 
objects and can be expressed as follows: 


P = 


E 

F • V + ^ T • a> 

Applied 

Forces 

Applied 

Torques 

E 

n 

F.^v,u.+ 53 T 

Applied 

Forces 

r=l Applied 

Torques 
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Applied Applied 
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B.3 Formulation of Equations of Motion for Planar Serial 
Link Manipulators 

A system S, consisting of a set of serially connected rigid links in planar configuration has 
a simple relationship relating link endpoint velocity to link basepoint velocity: 

^endpoint ^basepoint ^ ^.basepoint to endpoint 

if this is expressed using partial velocities, 

^endpoint _ ^basepoint _j_ j^bnk ^ ^.basepoint to endpoint 
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If we define unit vector xj to be along the link, from basepoint to endpoint, and unit 
vector yj to be perpendicular to xj and in the plane of the manipulator, then we can define 
z, a unit vector perpendicular to the plane of the manipulator as; 

A 

z = Xi X yi 

Vector z is the same for all links i. Now, the generalized speeds l..n and the angular 
velocities are related as follows; 


* = «.z 


and the endpoint to basepoint velocity relation for link t of length /, becomes; 

-Start to end i / v 

r — X| 

and the partial velocity relations along the chain from start i = 1 to end n, are as 
follows; 


V 


endi _ 
r 


{ 


start! 

/.XJ 

0 


for i = 1 ... r — 1 
for i = r 

for i = r + 1 . . . n 



for i = 1 ... r - 1 
for i ^ r 


Two additional generalized speeds, Un+i,n +2 describe the motion of a designated point 
one one of the chain links, for instance, the center of mass of the heaviest body, the ’robot 
body’. In order to calculate the partial velocities of the mass center of each link, we define 
an extra coordinate set, Xj , yj, and zj, such that; 


r 


start to endi 


Z 


/JXi 
X? X y\ 


^endpoint _ ^basepoint _j_ ^basepoint to endpoint 

then, if expressed using partial velocities, 

^endpoint _ ybasepoint y pbasepointtoendpoint 

yStarti for i = 1 ... r - 1 
/jXj for i = r 
0 for i = r + 1 . . . n 
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The contribution of body i to the inertia scalar m„, an element of the mass matrix M, 
can be determined as follows: 


(m„)‘ = m‘ vi* ■ v‘* + - r/‘* • oj'* 


for the planar system, this reduces to 




vr-vr 


for i / r 


/ \i J ^bodies i ^ 

(^rsj — S ^ • • {/im r 

Ebodies i < -K + 1/ for n = r = s 


where 




and 


, .linki _ 

ti) = U,Z 


the sum of the effects of all bodies, which build a complete inertia scalar. 


= m^vj* ■ v]* + m^vj* • v^* + . . . 

bodies i 

case r = s = j 

= vf • vj* + + . . . 

= (/>*)" + + 

= m> (r-y 

case r ^ s,k = max(r,s) 

= rn^ f-' Vk • vf 

+ (m^+^ + m''+2 + . . y^ndt . .^endk 

a similar derivation for the ’non-linear’ momentum scalar terms: 

(n„)’ = m’ 

where 

u?;* -H* =0 

In the planar configuration, no torques due to changes in orientation of the angular 
momentum ^ ector of any of the bodies occur. 

If we define 
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m. 


= m +m + 


+ m" 


where 


k = max(r,s) 

then the inertia and momentum scalars for the mass and non-linear coupling terms 
matrices can be expressed as: 


m„ = m;, " • vf " 

4-m*' (I'kyy) ■ " 

and if j = r = s 

+P/-'* 

similarly, the expression for the non-linear coupling term matrix scalars is: 


rirs — 

if s > r 
if r > s 


B.4 Control Specification 

This section deals with the means whereby control specifications can be expressed in terms 
of quantities expressed or derived in the previous chapters. We wish to be able to determine 
the Jacobian scalars which form a Jacobian matrix of the form: 

.^endpoint ^ ^^2 

The endpoint velocity can be expressed in terms of its partials as: 

^■4-2 

^endpoint „cndpoint 

r=l 

and therefore endpoint velocity can be expressed in terms of speeds along the inertial 
’x’ and ’y’ directions, that is, along unit vectors x and y: 


+m‘ ('JyDvr"*" 

Equations 
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,x endpoint 

n-h2 

= ^ • X U,. 


T=1 

,y endpoint 

n+2 

= • y Ur 


r=l 


the elements of the Jacobian are therefore: 


ju = 

> 2 , = 

if the endpoint is at the tip of the manipulator, then these partial velocities are exactly 
the same as the ones used in the dynamical derivations. 

The control specification, assumed here to be an acceleration specification for the end- 
point, is determined from the following expression: 

^endpoint _ ^^2 + jui..„+2 

the derivative of the Jacobian can be determined from quantities used in the dynamical 
equation formulation: 


3 - 2 . = ^endpoint, y 

B.5 Nonholonomic Constraints: Closed Chain 

In a dynamical system with nonholonomic constraints, the generalized speeds Ui..n are not 
independent, rather, one (or more) are dependent on the rest. In the system considered, a 
serial chain of bodies, this condition can arise when the two ends of the chain touch and 
are held together, either by a pin joint, or rigidly. The case of a pin constraint on the two 
dimensional manipulator, a nonholonomic constraint situation, will be analyzed, and the 
constraint equations wiU be expressed in terms of quantities used in the derivations for 
dynamical analysis. 

The constraint of endpoint closure is described by: 

_ yCndn 

expanding this into partial velocities. 
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It is evident that by dot multiplication with two vectors, this vector equation can be 
reduced to two scalar equations, for the two-dimensional manipulator we are studying. If 
we choose the vectors such that each is perpendicular to the partial velocity of the end 
bodies, then the two equations decouple and the last two generalized speeds can be solved 
for explicitly. Two candidate vectors to accomplish this are the derivatives of the partial 
velocity vectors of the last two links; v«“4n since these vectors are guaranteed 

to be perpendicular to their respective partial velocities. This is proven by the fact that 

.^body i _ ^body i ^ ^body i 

If we use these two vectors to separate the vector constraint equation into two scalar 
constraint equations, we get: 


starti 


■body 


Ur 


= E 


,end„ . - body i 


Ur 


r=l 


r=l 


we note that 


= 0 

starti .j,endn _ n 

M..n M..n ” ^ 


SO that the two constraint equations are: 


0 = 

r=l 

0 = Ev^v>, 

r=l 


These equations are decoupled for u\ and u„ and can be rearranged to pick out these 
terms explicitly: 


n— 1 



r=2 


such that the reduced set of independent speeds, can be expressed as: 

Ul..n = C«i..n+2 


where 
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E "-> 

r=2 M 


V"Vj 

v"-v" 

r=2 


for r = l..n 


if j - 1 = r Cj> = 1 J 

if j — 2 = r Cjr = 1 } for r = n + 1, n + 2 

Cjr = 0 } for all other j, r 

The constrained equations of motion can then be formulated as follows: 


where 


Mui..„ = -Nui ..„+2 + Fi..„ 


M = CMC^ 

N = C (n - MC^) 

F = CF 


[ 


r 


[ 


r 


r 




r 


I 


[ 


r 
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Appendix C 


Derivations of the Equations of 
Motion for LEAP Vehicle 

Warren Jasper. J 

C.l Definitions of the Generalized Speeds 

Using Kane’s method for deriving the equations of motion, the following list defines the 
generalized speeds. 

ui = -61 = 91 cos( 93 ) -f 92 sin(93) 

U2 = • 62 = 92 cos(93) - 9 i sin(93) 

U 3 = = 93 

tt4 = • fc = 93 + 94 

«5 = • fc = 93 + 94 + 95 

Us = • fc = 93 + 96 

ti? = • *: = 93 + 96 + 97 

U8 = • fc = 93 + 98 

Using these definitions for the generalized speed, the velocities of the points of interest 
become: 


= til 61 +74262 (C.l) 

_ uibi + U2 62 + ti 3 /i r2 (C.2) 

= til 61 + 1i2 62 + ti3 h 7*2 + ti4 /2 C2 - ti4 /3 Cl (C.3) 

■^v^* = til 61 + ti2 62 + ti3 /i r2 + ti4 /4 C2 (C.4) 


PRECEDING PAGE BLANK NOT FILMED 
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= uj hi 4 - U2 62 + U3 li r2 + U4 14 C2 + U5 Is d2 — le dj (C. 5 ) 

= Ui 61 + U2 62 4 - U3 ‘f'2 + ‘^4 U ^2 4 - Us I7 8,2 (C.6) 

= Ui 61 4 - U2 62 4 - li3 /g T’4 (C. 7 ) 

= Ui hi 4 - U2 &2 + U3 ^8 »*4 + Ue /g ej - “6 ^0 Cl (C.8) 

= ui hi + ti2 h2 4 - U3 /g t *4 4 - lie ^11 €2 (C. 9 ) 

= 111 hi 4 - 112 62 4 - 113 ^8 ^4 + ue /ii e2 4- U7 /12 /2 ~ “7 ^ 13 /i (C.IO) 

= til hi 4 - ti2h2 4 - ii3/8 7‘4 + ue^ii 62 4 - 117/14/2 (C.H) 

= 111 hi 4-112 h2 4-113 /is T’e (C. 12 ) 


C.1.1 Accelerations 

By differentiating the above velocities with respect to time in the reference frame A, the 
following eight accelerations of interest become: 

= Ui hi 4- ii2 h2 - ii 2 ti 3 hi 4 - iiiii 3 h 2 

^Op' — ill hi 4 - U 2 />2 - ^ 2^3 />1 -t- U1U3 ^>2 -b U3/1 7*2 -f U4h Cj - 

114/361 - 113^/1 T"! - 114^/261 - 114^/302 
^0'°’ = ill hi 4 - il2 62 - “2W3/>1 H- 71 lll 3 fr 2 -b 113/1 7*2 4 - 114/4 C2 4 - 

115/5 </2 - 115/6 «/i - 113^/1 7*1 - 114^/4 Cl - Us^/s i/i - Us^/e d 2 

= ill hi 4 - ii2 />2 - 112113 />i + 111113 h2 -b us/s 7*4 4 - ueh 62 - 

ue/ioci — — ue^/gci — 116^/1062 

= ill hi 4 - ii2 />2 - 112113 /»i -b 111113 h2 4 - 113/8 7*4 4 - uehi ^2 -b 

117/12/2 “ 117/13 / 1 - 113^/8 7*3 — ue^/ii Cl — 117^/12 /i ~ 117^/13 / 2 
= ill hi 4 - it2 />2 - 11211361 4 - 111113 h2 4 - 113/15 7-6 - 113^/15 rs 
= ill hi 4 - ii2 62 - 112II361 -b 111II362 -b ii 3 /i 7 * 2 -b ^4/4 62 -b 

il 5/7 c /2 - 113^/1 7*1 - 114^/4 Cl - 115^/7 di 

= ill hi 4 - ii2 62 - 112113 61 -b 111113 62 -b iizh 7*4 -b iie/ii 62 4 - 

^ihAfz ~ 113^/8 7’3 - Ue^/ll Cl — U 7 ^/i 4 /i 


C.1.2 Partial Velocities and Partial Angular Velocities 


The next step in the derivation of the equations of motion is to calculate the partial 
velocities and partial angular velocities where 


A P ^ 
= 




d 

dUr 

dUr 


(C.13) 

(C.14) 


The following is a table of the partial velocities and partial angular velocities for the robot. 
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r 

A^B 


A^C- 

AvP* 

A^D- 

A^F, 


1 

bi 

bi 

bi 

bi 

bi 

bi 


2 

b2 

62 

b2 

62 

62 

62 


3 

0 

/i r2 

h »*2 

h r2 

h r2 

h r2 


4 

0 

0 I2 C2 — I3 Cl 

I4C2 

I4C2 

I4 C2 


5 

0 

0 

0 

0 hd2 — h d\ 

h d2 


6 

0 

0 

0 

0 

0 

0 


7 

0 

0 

0 

0 

0 

0 

8 

0 

0 

0 

0 

0 

0 

r 

r 

rf® 

A^E- 

A^^ 

A^F- 

^j,Pe 

A^G- 

1 

1 


bi 

bi 

bi 

bi 

bi 

2 

1 


62 

62 

i>2 

62 

62 

3 


k r 4 

hr 4 

h r 4 

hr 4 

hs rs 

4 

1 


0 

0 

0 

0 

0 

5 

1 


0 

0 

0 

0 

0 

6 


0 

Ige2 — ho^l 

hi ^2 

hi ^2 

hlC2 

0 

7 


0 

0 

0 

112/2 — hsfi hif2 

0 

8 


0 

0 

0 

0 

0 

0 


Table C.l: The partial velocities 




0 

0 

k 

0 

0 

0 

0 

0 


0 

0 

0 

k 

0 

0 

0 

0 


0 

0 

0 

0 

k 

0 

0 

0 


0 

0 

0 

0 

0 

k 

0 

0 


0 

0 

0 

0 

0 

0 

k 

0 


0 

0 

0 

0 

0 

0 

0 

k 


Table C.2: The partial angular velocities 


C.l. 3 The Mass Matrix 

By combining terms, one can put the equations of motion into the form 

Mu = fj + KiT 
q = Tu 


(C.15) 

(C.16) 


where the mass matrix M is a symmetric positive definite matrix. 
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Z \ 

0 

22 

2354 + 

24C4 

25545 + 

26^45 

zise + 

28C6 

29S67 + 

2ioC67 

0 

0 

2l 

2ll 

2454 

23C4 

26545 — 

25C45 

2856- 

27C6 

2io567 — 

29C67 

0 

Z 2 

2ll 

2i2 

21354 + 

214C4 

215545 + 

216C45 

21756 + 

218C6 

219567 + 

220C67 

0 

zaSA + 

Z4C4 

2454 

23C4 

21354 + 

214C4 

221 

22255 + 

223C5 

0 

0 

0 

25S45 + 

zeCis 

25545 — 

25C45 

21554s + 

216C45 

22255 + 

223C5 

224 

0 

0 

0 

2756 + 

z&ce 

2856- 

ZlCe 

21756 + 

218^6 

0 

0 

225 

22657 + 

227C7 

0 

29567 + 

2ioC67 

210567 — 

29C67 

219567 + 

220^67 

0 

0 

22657 + 
227C7 

228 

0 

0 

0 

0 

0 

0 

0 

0 

229 . 


Where the Zi are constants and have the following definitions: 


(C.17) 


2l 

A 

22 

A 

23 

A 

24 

A 

25 

A 

26 

A 

27 

A 

28 

A 

29 

A 

2l0 

A 

2ll 

A 

212 

A 

2i3 

A 


mi + 7712 + m3 + 7714 + ms + me 

-(m2 + m3)/i sin( 0 i) - (m4 + m^ys sin(^2) “ ''Tiehs sin(03) 

m2/2 + m 3/4 

m 2 h 

Trials 

male 

—{ m^lQ + mshi ) 

—m^liQ 

-7775/12 

—msha 

(m2 m3)/i cos(^i) + (m4 + m5)/8cos(02) + mehs 005(^3) 

h + (7772 + m 3 )/i^ + (m 4 + ms)h^ + me/is^ 
m 2 /i/ 3 Cos(^i) - m 3 /i/ 4 sin(^i) — m2/i/2 sin(^i) 
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714 

A 

715 

A 

7i6 

A 

717 

A 

718 

A 

719 

A 

720 

A 

721 

A 

722 

A 

723 

A 

724 

A 

725 

A 

726 

A 

727 

A 

728 

A 

729 

A 


— 1712^1/3 sin(0i) — 7713/1/4 cos(0i) — 77x2/1/2^02(^1) 
77 x 3 / 1 /6Cos(fli) - sin(0i) 

-77x3/1/6 sin(di) - mslils cos {$ i ) 

- 77 x 4 / 8 / 10003 (^ 2 ) + rnslshi sin(^2) + ”^4^9 sin(52) 
17 x 4 / 8/10 sin(fl2) + ’ns/s/ii cos(02) + 17 x 4 / 8 / 9003 (^ 2 ) 
- 77 x 5 / 8 / 13003 ( 02 ) + 7n5/8/i2sin(02) 
nxs/s/ia sin( 02 ) + oos( 02 ) 

/2 + 17X2 (/2^ 4-/3^) + 

-m^Ule 

13 + Tnz{h'^ + l^) 

14 + m4{l^ + /lo^) + 1175/11^ 

- 77 x 5 / 11/13 

77 x 5 / 11/12 

/s + 1175 (/i 2^ + /l3^) 

h 


The non-linear term3 in u, are found in the vector //, and with the the definition3 of the 
Zi from above, they become: 


//(I) 

//( 2 ) 

//( 3 ) 

//( 4 ) 

//( 5 ) 

//( 6 ) 

//( 7 ) 

//( 8 ) 


U2U3Z1 -I- U3^2ii -I- 1X4^(2:454 - Z3C4) -f U 5 ^( 2:6545 - -75045) + 

U6^(7:8'S6 — + ^7^(^10567 — ^gCex) 

-XilU32i - 1 x 3^22 - U4^(-7324 + ^ 404 ) - 176^(^5245 + ^ 6045 ) - 
«6^(-7726 + -T-sCe) “ «7^(-79267 + -7l0O67) 

— IX1IX32II -h U2U3Z2 + IX4^(-2:i424 — - 713C4) + 175^(^16245 — -715045) + 
176^(71826 — 7 i 7 C 6 ) -f 177^(720267 — 719C67) 

-1X1173(2424 - 73C4) + U2U3(2354 -|- 24C4) - 1X3^(71424 - 213C4) + 

175^(72325 — -72205) 

— 171173(76245 — 75 C 45 ) + 172173(75245 + 25045 ) — 1 X 3^(216245 — 215 C 45 ) — 
174^(72325 — 722O5) 

— 171173(7826 — 7705) -f UzUs^ZjSq + ZgCe) — 173^(71826 — 7 l 7 C 6 ) -|- 

177^(72727 — 726O7) 

-1X1173(210267 — 79C67) 4 - 1X2173(79267 + 710O67) - 173^(220267 — 719C67) “ 
176^(72727 — ^ 2607 ) 

0 
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The vector iifiT is given by 

■ 0 0 0 0 0 

0 0 0 0 0 

-1 0-1 0 -1 

^ 1-1000 
0 10 0 0 

001-10 
0 0 0 1 0 

0 0 0 0 1 

Where the rj are input torques from the five motors. 

The T matrix is 


COs(93) 

- sin(g 3 ) 

0 

0 

0 

0 

0 

0 

sin{qz) 

cosiqz) 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

-1 

1 

0 

0 

0 

0 

0 

0 

0 

-1 

1 

0 

0 

0 

0 

0 

-1 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

-1 

1 

0 

0 

0 

-1 

0 

0 

0 

0 

1 


n 

T2 

T3 

T4 

Ts 


(C.18) 


C.1.4 Non-Holonomic Constraint Equations 

When both arms of the robot are grasping the bar, the number of degrees of freedom of the 
system drop from eight to four. At the tip of each arm, there two non-holonomic or velocity 
constraints which much be satisfied to insure the closed kinematic-chain configuration. The 
four constraint equations are: 


= 0 (C.19) 

= 0 (C.20) 

= 0 (C.21) 

= 0 (C.22) 


Substituting Eqn. (C.6) and Eqn. (C.ll) yields the following set of non-holonomic con- 
straint equations 


Uf — ArgUg 


(C.23) 
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U 4 

U 5 

U6 

U7 


cos(74+95) 

8111 ( 94 + 95 ) 

/l sin(94 +95-^1 ) 

<4 siii( 9 s) 

U sin( 9 s) 

U sin(95) 


cosfq^) _ 8in(9j) fi sin(i?<-gi ) 

h sin( 95 ) I 7 sin(fs) h sin(? 5 ) 


cos( 96+97) 
ill sin(97) 

sin(96+97) 
111 sin(97) 

Is 8111 ( 96 + 77 - 62 ) 
In sin( 97 ) 

C0S(96) 

8in(96) 

Is 8111 ( 96 - 62 ) 

ii4 8in((j7) 

/l4 sin( 97 ) 

/i 4 sin(97) 


Ul 

U2 

W 3 


Differentiating the velocities in Eqn. (C. 6 ) and Eqn. (C.ll) with respect to time in reference 
frame A and dotting the acceleration vectors with orthogonal unit vectors 61 and 62 gives 
the following set of equations which expresses in terms of Ua,u and q. 


Ur — ArjUi brs 


(C.24) 


U 4 

U5 

U6 

U 7 


cos(94+95) 

U sin(9s) 

Bin(94+9s) 

U sin(95) 

h sin(94+9s-6i) 

I4 sin(95) 


cos(94) 

h 8in(9s) 

8111(94) 

I7 sin(95) 

1] sin(94-6i) 

I7 8111(95) 


ill 

cos( 96+97) 
111 sin(97) 

sin(96+97) 
111 sin(97) 

Is 8111(96+97-62) 

111 sin(97) 


U2 

. ^3 . 

COs(96) 

8in(96) 

Is 8in(96-62) 


/J4 sin(97) 

Il4 8111(97) 

/i4 sin(97) -1 



UiU 3 8 m(gt+qi)-U 2 U 3 COs(g 4 -fg 5 )-U 3 ^<l COs(q 4 + (? 5 -gl cos(? 5 )+U 5^/7 

I* Bin(?5) 


+ 


-Ulti3 8in(g4)+«2«3 COs(g4)4-U3^/] C06(74-gi)^U4^^-VS^^7 <= 08 ( 95 ) 

h 8in(?s) 

-unt3 8in(<?6+q7) + «2«3COs((;6-fg7)+U3^l8 COs(?6+?7^P2)+U6^ln COs(g7) + «7^1:4 

<lisin(?7) 


UlU 3 8in(g6)-«2tt3 C«»(g6)-tl3^/g c«»(q6-92)-tt6^hl-UT^h4 coeiqr) 
In sin(q7) 


With this constraint equation, one can derive the reduced set of the equations of motion 
by partitioning Eqn. (C.15) and adjoining the constraint equation as follows 


Afii Afi2 

MJ 2 M 22 


U, 

Ur 



(C.25) 


[Mii + Mi2Ara + (Afl2-Ars)^ + ■A^Ai’22A.rjj U* = 

[fl. + A^sflr - - A^,,M 22 hrs\ + \Ku + r 


(C.26) 
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C.1.5 Force Constraint 

The is an alternate but equivalent formulation of the equations of motions which do not 
involve solving for the non-holonomic constraints, but rather impose a force boundary 
condition at the tips to insure that the velocity at the tips of the arms is zero when the 
arms are grasping the bar. If Eqn. (C.15) is modified to include forces at the tip we get 

Mu = fi + KiT + K2fT (C.27) 

Where fx is a four vector representing the normal and tangential forces exerted by the 
bar on the arms at points P 5 and Pg. 

cos(g4 + 95 ) - sin(q4 + 95 ) - cos(g 6 + 9?) sin(q 6 + Qr) 

sin(94 + 95) cos(g4 + gs) -sin(g 6 + 97) -cos(g 6 + g7) 

/i sin(94 + 95 - ^1) /i cos(94 + 95 - ^ 1) -/8sin(96 + 97 - ^2) -hcos{qe + q7 - O 2 ) 
-/4sin(95) -/4 Cos(95) 0 0 

0 -h 0 0 

0 0 -lnsin(97) -In 005 ( 97 ) 

0 0 0 -li4 

0 0 0 0 
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Rewriting Eqn. (C.23), yields 


Au = 0 


(C.28) 


1 0 — /isin(^i) 

0 1 /icos(^i) 

1 0 — /8sin(02) 

0 1 /8COS(02) 


/4sin(g4) /7sin(94 + 95) 

-I4 005(94) -I7 cos(g4 + 95 


•I I -JO/ 

■I7 cos(94 + 95) 
0 
0 


-111 sin(96) -/i4 sin(96 + 97) 
hi cos(96) /i4 cos (96 + 97) 


Differentiating Eqn. (C. 28 ) with respect to time in reference frame A yields 

Au = 6 

U2U3 + us^h cos(0i) - ^4^/4 005(94) - us'^h cos(94 + 95) 

^ _ -U1U3 + U 3 ^h sin( 0 i) - U42/4sin(94) - Us^hsinlq^ + 95) 

~ U2U3 + U3‘^ls 005(62) + UG^hi cos(qe) + U7^/i4 cos(96 + 97) 

. -U1U3 + sin(^2) + ue'^hi sin(96) + n7^^4 sin(96 + 97) . 

Using Eqn. (C. 27 ) and Eqn. (C. 29 ), the residtant forces exerted by the bar on the 
points Ps and Pg are 

/r = [AM-'^K2]-^[b - AM-^fi - AM-'^Kir] 


(C.29) 


arms at 


(C.30) 


C.l. 6 Some Properties of the Center of Mass 

In controlling the robot, there are some properties of the center of mass that are of interest. 
They are the position, velocity and acceleration of the center of mass. 

= 9iOi + 9202+ 

(211 - Z3C4 + Z4S4 - 25C45 + 26^45 “ + -^s^e - Z9C67 + ^lo^er)/^! 61 

— (Z2 + Z 3 S 4 + ^^4^4 + ^5^45 + 26C45 + 2756 + 28C6 + ZgSe? + Z\oCq7)/Z\ 62 
The velocity of the center of mass is given by: 

= (21^1 + 22U3 + (2354 + 24C4)U4 + 

(2^5545 + Z6C45)U3 + (2756 + 28C6)U6 + (2gS67 + ^^lOCe?)^?)/^!^! + 

(21U2 + znU3 - (23C4 - 2454)^4 - 

(■25C45 - ^6545)«5 “ (27C6 “ Z8S6)ue - {zgCe7 “ 2l0567)U7)/2l&2 
The acceleration of the center of mass is given by: 

- //(1)| 61 + |EM(2,t>(0 - //(2)| 62 

Once the equations of motions are derived, the kinetic energy is easily calculated as 

j 8 8 

Kinetic Energy = u(i)M(i,j)u{j) 

^ .=1 j=i 


[ 


r 


r 



r 




[ 


r 
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